#include "position_control.h"
#include "PID.h"

#define HeightRateIoutLimit 120
#define HeightRateLimitMax 300
#define HeightRateLimitMin (-100)

static volatile float TatHeight=0;	//临时目标高度

PIDOut pidHeightRate,pidHeight,pidXRate,pidYRate,pidX,pidY;

void Position_PID_Init(void)
{
	pidHeightRate.iOut_limit_max=HeightRateIoutLimit;
	pidHeightRate.iOut_limit_min=-HeightRateIoutLimit;
	pidHeightRate.limit_max=HeightRateLimitMax;
	pidHeightRate.limit_min=HeightRateLimitMin;
	
	pidXRate.dOut_limit_max=3;
	pidXRate.dOut_limit_min=-3;
	pidXRate.iOut_limit_max=2;
	pidXRate.iOut_limit_min=-2;
	pidXRate.limit_max=10;
	pidXRate.limit_min=-10;
	
	pidYRate.dOut_limit_max=2;
	pidYRate.dOut_limit_min=-2;
	pidYRate.iOut_limit_max=2;
	pidYRate.iOut_limit_min=-2;
	pidYRate.limit_max=10;
	pidYRate.limit_min=-10;	

}


/**
* @Description 位置内环pid控制
 */
void Position_Inner_Control(void)
{
	static unsigned int Position_InnertPre = 0;
	
	/************************ z轴速度环 ************************/
	//对手柄的值缩放、并计算误差 = 期望z轴速度 - 实际z轴速度
	Para_Info.HeightRate_Erro.Erro = (0.005f * Control_Info.hV - RT_Info.LASER_Alt_V);
	
	pid_calculate(&Para_Info.HeightRate_Erro, &Position_InnertPre, &Para_Info.HeightRate, &pidHeightRate);
	
	/************************ x方向速度环 ************************/
	static float lastX_Value = 0;
	//手柄控制速度为目标值，减去当前速度值
	Para_Info.rateX_Erro.Erro = (float)(0.005f * Control_Info.pV - RT_Info.FlowX_V);
	
	pid_calculate(&Para_Info.rateX_Erro,&Position_InnertPre,&Para_Info.rateX,&pidXRate);
	
	//pid计算结果取0.5权重
	Target_Info.Pitch = pidXRate.value;
	Target_Info.Pitch = (float)0.5*Target_Info.Pitch + (float)0.5*lastX_Value;
	lastX_Value = Target_Info.Pitch;
	
	/************************ y方向速度内环 ************************/
	static float lastY_Value = 0;
	//手柄控制速度为目标值，减去当前速度值
	Para_Info.rateY_Erro.Erro = (float)(0.005f * Control_Info.rV - RT_Info.FlowY_V);
	
	pid_calculate(&Para_Info.rateY_Erro,&Position_InnertPre,&Para_Info.rateY,&pidYRate);
	
	//pid计算结果取0.5权重
	Target_Info.Roll = pidYRate.value;
	Target_Info.Roll = (float)0.5*Target_Info.Roll + (float)0.5*lastY_Value;
	lastY_Value = Target_Info.Roll;	
}

#if 0
void Position_Outer_Control(void)
{
	static unsigned int Position_OutertPre=0;
	/*高度外环控制*/
	static float Takeoff_Weight = 0.03;	//每次上升or下降的步幅
	static float Landing_Weight = 0.005;
	
	if(FlightControl.landFlag==1)
	{
		TatHeight=TatHeight - Landing_Weight;		
		if(RT_Info.US100_Alt<0.10f)
		{
			FlightControl.OnOff = Drone_Land;
		}
	}
	else
	{
		//上位机的目标高度大于当前的目标高度，则缓慢上升
		if(TatHeight < Target_Info.Height)
		{
			TatHeight = TatHeight + Takeoff_Weight;
		}
		else
		{
			//下降或保持
			TatHeight = Target_Info.Height;
		}
	}
	
	float heightErro = TatHeight - RT_Info.US100_Alt;
	pidHeight.feedforwardOut = pidHeight.feedforwardOut * heightErro;
	Para_Info.Height_Erro.Erro=heightErro;
	
	pid_calculate(&Para_Info.Height_Erro,&Position_OutertPre,&Para_Info.Height,&pidHeight);
	#if 0
	/* 水平x、y方向p控制 */
	float X_Erro = RT_Info.FlowX,Y_Erro = RT_Info.FlowY;
	float x_kp=3.5f,y_kp=3.5f;
	
	pidX.value=x_kp*X_Erro;
	pidY.value=y_kp*Y_Erro;
	#endif
}

#endif
